#include "SimulationManager.h"

#include "RandomParticleVehicleDataPredictor.h"
#include "ImageLandmarksVehicleWeightCalculator.h"
#include "ImageLandmarksLikelyhoodEvaluator.h"
#include "utils.h"

SimulationManager::SimulationManager(void)
{
}

SimulationManager::~SimulationManager(void)
{
}

void SimulationManager::run() {

	setUpWindow();
	initializeGroundTruthVehicleData();
	initializeParticleFilter();

	time = 0;

	while (pWinPF->isOpen() && !mrpt::system::os::kbhit()) {

		updateGroundTruthVehicleData();

		simulateNoisyObservation();

		processWithPF();

		showPFState();

		draw();

		// Delay:
		mrpt::system::sleep((int)(deltaTime * 1000));

		time += deltaTime;
	}
}

void SimulationManager::initializeGroundTruthVehicleData() {
	groundTruthVehicle = initialVehicleData;
}

void SimulationManager::updateGroundTruthVehicleData() {
	groundTruthVehicle.x += deltaTime * groundTruthVehicle.vx;
}

void SimulationManager::simulateGroundTruthObservation() {

	Camera1DExternal camExternal(cvPoint2D32f(groundTruthVehicle.x, groundTruthVehicle.y), groundTruthVehicle.w);

	Camera1D cam(camInternal, camExternal);

	groundTruthObservation = predictImageLandmarks(cam, landmarkMap);

}

void SimulationManager::simulateNoisyObservation() {
	
	simulateGroundTruthObservation();

	noisyObservationPtr = CObservationImageLandmarksPtr(new CObservationImageLandmarks(groundTruthObservation));
	
}

void SimulationManager::processWithPF() {
	CSensoryFrame SF;
	SF.insert( noisyObservationPtr );  // memory freed by SF.
	PF.executeOn(*pParticleFilter, NULL,&SF);  // Process in the PF
}

void SimulationManager::showPFState() {
	cout << "Particle filter ESS: " << pParticleFilter->ESS() << endl;
}

void SimulationManager::draw() {
	drawPfState();
	drawGroundTruth();
	drawMap();
}

void SimulationManager::drawPfState() {

	size_t i,N = pParticleFilter->m_particles.size();
	vector_float parts_x(N),parts_y(N);
	for (i = 0; i < N; i++) {
		parts_x[i] = pParticleFilter->m_particles[i].d->x;
		parts_y[i] = pParticleFilter->m_particles[i].d->y;
	}

	pWinPF->plot( parts_x, parts_y, "b.2", "particles" );
}

void SimulationManager::drawGroundTruth() {
	pWinPF->plot( vector_float(1,groundTruthVehicle.x), vector_float(1,groundTruthVehicle.y),"k.8","plot_GT");
}

void SimulationManager::drawMap() {

	size_t i = 0;
	size_t N = landmarkMap.landmarks.size();

	vector_float parts_x(N), parts_y(N);

	for (i = 0; i < N; i++) {
		parts_x[i] = landmarkMap.landmarks[i].second.x;
		parts_y[i] = landmarkMap.landmarks[i].second.y;
	}

	pWinPF->plot(parts_x, parts_y, "r.8", "landmarks");
}

void SimulationManager::setUpWindow() {

	pWinPF = new CDisplayWindowPlots("Tracking - Particle Filter",450,400);

	pWinPF->setPos(480,10);

	pWinPF->axis(-2,20,-10,10);  

	pWinPF->axis_equal();

}

void SimulationManager::initializeParticleFilter() {

	// Create PF
	// ----------------------
	CParticleFilter::TParticleFilterOptions	PF_options;
	PF_options.adaptiveSampleSize = false;
	PF_options.PF_algorithm = CParticleFilter::pfStandardProposal;
	PF_options.resamplingMethod = CParticleFilter::prSystematic;

	PF.m_options = PF_options;


	pParticleFilter = new CBearingOnlyParticleFilter();
	pParticleFilter->initializeParticles(numParticles);

	pParticleFilter->setInitialValues(initialVehicleData);

	CRandomParticleVehicleDataPredictor* pPredictor = new CRandomParticleVehicleDataPredictor(stdDev);
	pParticleFilter->setParticleDataPredictor(pPredictor);

	CImageLandmarksVehicleWeightCalculator* pWeightCalculator = new CImageLandmarksVehicleWeightCalculator(&landmarkMap, &camInternal, new CImageLandmarksLikelihoodEvaluator());
	pParticleFilter->setParticleDataWeightCalculator(pWeightCalculator);

}

